/******************************************************************************
 * Copyright 2022 The AIR Authors. All Rights Reserved.
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *****************************************************************************/

#include <string>
#include "v2x-asn-msgs-internal.h"
bool speed_limit_asn2pb(const RegulatorySpeedLimit *sl_asn,
                        ::v2xpb::asn::RegulatorySpeedLimit *sl_pb) {
  if (!sl_asn || !sl_pb) {
    LOG(ERROR) << "NULLPTR";
    return false;
  }

  sl_pb->set_speed_limit_type(sl_asn->type);
  sl_pb->set_speed(sl_asn->speed);

  return true;
}

bool speed_limit_pb2asn(const ::v2xpb::asn::RegulatorySpeedLimit &sl_pb,
                        RegulatorySpeedLimit *sl_asn) {
  if (!sl_asn) {
    LOG(ERROR) << "NULLPTR";
    return false;
  }

  if (sl_pb.has_speed_limit_type()) {
    sl_asn->type = (e_SpeedLimitType)(sl_pb.speed_limit_type());
  }

  if (sl_pb.has_speed()) {
    sl_asn->speed = sl_pb.speed();
  }

  return true;
}

bool movement_asn2pb(const Movement *mv_asn, ::v2xpb::asn::Movement *mv_pb) {
  if (!mv_asn || !mv_pb) {
    LOG(ERROR) << "NULLPTR";
    return false;
  }

  if (mv_asn->remoteIntersection.region) {
    mv_pb->set_region(*mv_asn->remoteIntersection.region);
  }
  mv_pb->set_id(mv_asn->remoteIntersection.id);

  if (mv_asn->phaseId) {
    mv_pb->set_phase_id(*mv_asn->phaseId);
  }

  return true;
}

bool movement_pb2asn(const ::v2xpb::asn::Movement &mv_pb, Movement *mv_asn) {
  if (!mv_asn) {
    LOG(ERROR) << "NULLPTR";
    return false;
  }

  if (mv_pb.has_region()) {
    mv_asn->remoteIntersection.region = ASN1(RoadRegulatorID)::create_tp();
    *mv_asn->remoteIntersection.region = mv_pb.region();
  }

  if (mv_pb.has_id()) {
    mv_asn->remoteIntersection.id = mv_pb.id();
  }

  if (mv_pb.has_phase_id()) {
    mv_asn->phaseId = ASN1(PhaseID)::create_tp();
    *mv_asn->phaseId = mv_pb.phase_id();
  }

  return true;
}

bool lane_attri_asn2pb(const LaneAttributes *la_asn,
                       ::v2xpb::asn::LaneAttributes *la_pb) {
  if (!la_asn || !la_pb) {
    LOG(ERROR) << "NULLPTR";
    return false;
  }

  if (la_asn->shareWith) {
    ASN1_HOLDER(LaneSharing) const share_asn(la_asn->shareWith);
    std::string str("");
    int size = la_asn->shareWith->size - la_asn->shareWith->bits_unused;
    for (int i = 0; i < size; i++) {
      str.push_back((share_asn.test_bit(i)) ? '1' : '0');
    }
    la_pb->set_share_with(str);
  }

  la_pb->set_present(la_asn->laneType.present);

  if (la_asn->laneType.present == LaneTypeAttributes_PR_vehicle) {
    ASN1_HOLDER(LaneAttributes_Vehicle)
    const string_asn(&la_asn->laneType.choice.vehicle);
    int size = la_asn->laneType.choice.vehicle.size -
               la_asn->laneType.choice.vehicle.bits_unused;
    std::string str("");
    for (int i = 0; i < size; i++) {
      str.push_back((string_asn.test_bit(i)) ? '1' : '0');
    }
    la_pb->set_value(str);
  }

  if (la_asn->laneType.present == LaneTypeAttributes_PR_crosswalk) {
    ASN1_HOLDER(LaneAttributes_Crosswalk)
    const string_asn(&la_asn->laneType.choice.crosswalk);
    int size = la_asn->laneType.choice.crosswalk.size -
               la_asn->laneType.choice.crosswalk.bits_unused;
    std::string str("");
    for (int i = 0; i < size; i++) {
      str.push_back((string_asn.test_bit(i)) ? '1' : '0');
    }
    la_pb->set_value(str);
  }

  if (la_asn->laneType.present == LaneTypeAttributes_PR_sidewalk) {
    ASN1_HOLDER(LaneAttributes_Sidewalk)
    const string_asn(&la_asn->laneType.choice.sidewalk);
    int size = la_asn->laneType.choice.sidewalk.size -
               la_asn->laneType.choice.sidewalk.bits_unused;
    std::string str("");
    for (int i = 0; i < size; i++) {
      str.push_back((string_asn.test_bit(i)) ? '1' : '0');
    }
    la_pb->set_value(str);
  }

  if (la_asn->laneType.present == LaneTypeAttributes_PR_median) {
    ASN1_HOLDER(LaneAttributes_Barrier)
    const string_asn(&la_asn->laneType.choice.median);
    int size = la_asn->laneType.choice.median.size -
               la_asn->laneType.choice.median.bits_unused;
    std::string str("");
    for (int i = 0; i < size; i++) {
      str.push_back((string_asn.test_bit(i)) ? '1' : '0');
    }
    la_pb->set_value(str);
  }

  if (la_asn->laneType.present == LaneTypeAttributes_PR_striping) {
    ASN1_HOLDER(LaneAttributes_Striping)
    const string_asn(&la_asn->laneType.choice.striping);
    int size = la_asn->laneType.choice.striping.size -
               la_asn->laneType.choice.striping.bits_unused;
    std::string str("");
    for (int i = 0; i < size; i++) {
      str.push_back((string_asn.test_bit(i)) ? '1' : '0');
    }
    la_pb->set_value(str);
  }

  if (la_asn->laneType.present == LaneTypeAttributes_PR_trackedVehicle) {
    ASN1_HOLDER(LaneAttributes_TrackedVehicle)
    const string_asn(&la_asn->laneType.choice.trackedVehicle);
    int size = la_asn->laneType.choice.trackedVehicle.size -
               la_asn->laneType.choice.trackedVehicle.bits_unused;
    std::string str("");
    for (int i = 0; i < size; i++) {
      str.push_back((string_asn.test_bit(i)) ? '1' : '0');
    }
    la_pb->set_value(str);
  }

  if (la_asn->laneType.present == LaneTypeAttributes_PR_parking) {
    ASN1_HOLDER(LaneAttributes_Parking)
    const string_asn(&la_asn->laneType.choice.parking);
    int size = la_asn->laneType.choice.parking.size -
               la_asn->laneType.choice.parking.bits_unused;
    std::string str("");
    for (int i = 0; i < size; i++) {
      str.push_back((string_asn.test_bit(i)) ? '1' : '0');
    }
    la_pb->set_value(str);
  }

  return true;
}

bool lane_attri_pb2asn(const ::v2xpb::asn::LaneAttributes &la_pb,
                       LaneAttributes *la_asn) {
  if (!la_asn) {
    LOG(ERROR) << "NULLPTR";
    return false;
  }

  if (la_pb.has_share_with()) {
    const auto &str = la_pb.share_with();
    la_asn->shareWith = ASN1(LaneSharing)::create_tp();
    ASN1_HOLDER(LaneSharing) share_asn(la_asn->shareWith);
    share_asn.init();
    for (int i = 0; i < str.size(); i++) {
      share_asn.set_bit(i, (str[i] == '1') ? true : false);
    }
  }

  la_asn->laneType.present = (LaneTypeAttributes_PR)la_pb.present();

  if (la_asn->laneType.present == LaneTypeAttributes_PR_vehicle &&
      la_pb.has_value()) {
    const auto &str = la_pb.value();
    ASN1_HOLDER(LaneAttributes_Vehicle)
    string_asn(&la_asn->laneType.choice.vehicle);
    string_asn.init();
    for (int i = 0; i < str.size(); i++) {
      string_asn.set_bit(i, (str[i] == '1') ? true : false);
    }
  }

  if (la_asn->laneType.present == LaneTypeAttributes_PR_crosswalk &&
      la_pb.has_value()) {
    const auto &str = la_pb.value();
    ASN1_HOLDER(LaneAttributes_Crosswalk)
    string_asn(&la_asn->laneType.choice.crosswalk);
    string_asn.init();
    for (int i = 0; i < str.size(); i++) {
      string_asn.set_bit(i, (str[i] == '1') ? true : false);
    }
  }

  if (la_asn->laneType.present == LaneTypeAttributes_PR_bikeLane &&
      la_pb.has_value()) {
    const auto &str = la_pb.value();
    ASN1_HOLDER(LaneAttributes_Bike)
    string_asn(&la_asn->laneType.choice.bikeLane);
    string_asn.init();
    for (int i = 0; i < str.size(); i++) {
      string_asn.set_bit(i, (str[i] == '1') ? true : false);
    }
  }

  if (la_asn->laneType.present == LaneTypeAttributes_PR_sidewalk &&
      la_pb.has_value()) {
    const auto &str = la_pb.value();
    ASN1_HOLDER(LaneAttributes_Sidewalk)
    string_asn(&la_asn->laneType.choice.sidewalk);
    string_asn.init();
    for (int i = 0; i < str.size(); i++) {
      string_asn.set_bit(i, (str[i] == '1') ? true : false);
    }
  }

  if (la_asn->laneType.present == LaneTypeAttributes_PR_median &&
      la_pb.has_value()) {
    const auto &str = la_pb.value();
    ASN1_HOLDER(LaneAttributes_Barrier)
    string_asn(&la_asn->laneType.choice.median);
    string_asn.init();
    for (int i = 0; i < str.size(); i++) {
      string_asn.set_bit(i, (str[i] == '1') ? true : false);
    }
  }

  if (la_asn->laneType.present == LaneTypeAttributes_PR_striping &&
      la_pb.has_value()) {
    const auto &str = la_pb.value();
    ASN1_HOLDER(LaneAttributes_Striping)
    string_asn(&la_asn->laneType.choice.striping);
    string_asn.init();
    for (int i = 0; i < str.size(); i++) {
      string_asn.set_bit(i, (str[i] == '1') ? true : false);
    }
  }

  if (la_asn->laneType.present == LaneTypeAttributes_PR_trackedVehicle &&
      la_pb.has_value()) {
    const auto &str = la_pb.value();
    ASN1_HOLDER(LaneAttributes_TrackedVehicle)
    string_asn(&la_asn->laneType.choice.trackedVehicle);
    string_asn.init();
    for (int i = 0; i < str.size(); i++) {
      string_asn.set_bit(i, (str[i] == '1') ? true : false);
    }
  }

  if (la_asn->laneType.present == LaneTypeAttributes_PR_parking &&
      la_pb.has_value()) {
    const auto &str = la_pb.value();
    ASN1_HOLDER(LaneAttributes_Parking)
    string_asn(&la_asn->laneType.choice.parking);
    string_asn.init();
    for (int i = 0; i < str.size(); i++) {
      string_asn.set_bit(i, (str[i] == '1') ? true : false);
    }
  }

  return true;
}

bool map_node_id_asn2pb(const NodeReferenceID_t *id_asn,
                        ::v2xpb::asn::MapNode_ID *id_pb) {
  if (!id_asn || !id_pb) {
    LOG(ERROR) << "NULLPTR";
    return false;
  }
  if (nullptr != id_asn->region) {
    id_pb->set_region(static_cast<int>(*id_asn->region));
  }
  id_pb->set_id(static_cast<int>(id_asn->id));
  return true;
}
bool map_node_id_pb2asn(const ::v2xpb::asn::MapNode_ID &id_pb,
                        NodeReferenceID_t *id_asn) {
  if (!id_asn) {
    LOG(ERROR) << "NULLPTR";
    return false;
  }
  if (id_pb.has_region()) {
    id_asn->region = ASN1(RoadRegulatorID)::create_tp();
    *id_asn->region = id_pb.region();
  }
  id_asn->id = id_pb.id();
  return true;
}
//
bool map_allowed_maneuvers_asn2pb(
    const AllowedManeuvers_t *maneuvers_asn,
    google::protobuf::RepeatedField<int> *maneuvers_pb) {
  if (!maneuvers_asn || !maneuvers_pb) {
    LOG(ERROR) << "NULLPTR";
    return false;
  }
  ASN1_HOLDER(AllowedManeuvers) const maneuvers(maneuvers_asn);

  if (maneuvers.test_bit(AllowedManeuvers_maneuverStraightAllowed)) {
    maneuvers_pb->Add(v2xpb::asn::MapLane_AllowedManeuver_STRAIGHT);
  }
  if (maneuvers.test_bit(AllowedManeuvers_maneuverLeftAllowed)) {
    maneuvers_pb->Add(v2xpb::asn::MapLane_AllowedManeuver_LEFT);
  }
  if (maneuvers.test_bit(AllowedManeuvers_maneuverRightAllowed)) {
    maneuvers_pb->Add(v2xpb::asn::MapLane_AllowedManeuver_RIGHT);
  }
  if (maneuvers.test_bit(AllowedManeuvers_maneuverUTurnAllowed)) {
    maneuvers_pb->Add(v2xpb::asn::MapLane_AllowedManeuver_U_TURN);
  }
  if (maneuvers.test_bit(AllowedManeuvers_maneuverLeftTurnOnRedAllowed)) {
    maneuvers_pb->Add(v2xpb::asn::MapLane_AllowedManeuver_LEFT_ON_RED);
  }
  if (maneuvers.test_bit(AllowedManeuvers_maneuverRightTurnOnRedAllowed)) {
    maneuvers_pb->Add(v2xpb::asn::MapLane_AllowedManeuver_RIGHT_ON_RED);
  }
  if (maneuvers.test_bit(AllowedManeuvers_maneuverLaneChangeAllowed)) {
    maneuvers_pb->Add(v2xpb::asn::MapLane_AllowedManeuver_LANE_CHANGE);
  }
  if (maneuvers.test_bit(AllowedManeuvers_maneuverNoStoppingAllowed)) {
    maneuvers_pb->Add(v2xpb::asn::MapLane_AllowedManeuver_NO_STOPPING);
  }
  if (maneuvers.test_bit(AllowedManeuvers_yieldAllwaysRequired)) {
    maneuvers_pb->Add(
        v2xpb::asn::MapLane_AllowedManeuver_YIELD_ALWAYS_REQUIRED);
  }
  if (maneuvers.test_bit(AllowedManeuvers_goWithHalt)) {
    maneuvers_pb->Add(v2xpb::asn::MapLane_AllowedManeuver_GO_WITH_HALT);
  }
  if (maneuvers.test_bit(AllowedManeuvers_caution)) {
    maneuvers_pb->Add(v2xpb::asn::MapLane_AllowedManeuver_CAUTION);
  }
  if (maneuvers.test_bit(AllowedManeuvers_reserved1)) {
    maneuvers_pb->Add(v2xpb::asn::MapLane_AllowedManeuver_RESERVED);
  }
  return true;
}

bool map_allowed_maneuvers_pb2asn(
    const google::protobuf::RepeatedField<int> &maneuvers_pb,
    AllowedManeuvers_t *maneuvers_asn) {
  if (!maneuvers_asn) {
    LOG(ERROR) << "NULLPTR";
    return false;
  }
  ASN1_HOLDER(AllowedManeuvers) maneuvers(maneuvers_asn);
  maneuvers.init();
  for (const auto &maneuver_pb : maneuvers_pb) {
    switch (maneuver_pb) {
      case v2xpb::asn::MapLane_AllowedManeuver_STRAIGHT:
        maneuvers.set_bit(AllowedManeuvers_maneuverStraightAllowed);
        break;
      case v2xpb::asn::MapLane_AllowedManeuver_LEFT:
        maneuvers.set_bit(AllowedManeuvers_maneuverLeftAllowed);
        break;
      case v2xpb::asn::MapLane_AllowedManeuver_RIGHT:
        maneuvers.set_bit(AllowedManeuvers_maneuverRightAllowed);
        break;
      case v2xpb::asn::MapLane_AllowedManeuver_U_TURN:
        maneuvers.set_bit(AllowedManeuvers_maneuverUTurnAllowed);
        break;
      case v2xpb::asn::MapLane_AllowedManeuver_LEFT_ON_RED:
        maneuvers.set_bit(AllowedManeuvers_maneuverLeftTurnOnRedAllowed);
        break;
      case v2xpb::asn::MapLane_AllowedManeuver_RIGHT_ON_RED:
        maneuvers.set_bit(AllowedManeuvers_maneuverRightTurnOnRedAllowed);
        break;
      case v2xpb::asn::MapLane_AllowedManeuver_LANE_CHANGE:
        maneuvers.set_bit(AllowedManeuvers_maneuverLaneChangeAllowed);
        break;
      case v2xpb::asn::MapLane_AllowedManeuver_NO_STOPPING:
        maneuvers.set_bit(AllowedManeuvers_maneuverNoStoppingAllowed);
        break;
      case v2xpb::asn::MapLane_AllowedManeuver_YIELD_ALWAYS_REQUIRED:
        maneuvers.set_bit(AllowedManeuvers_yieldAllwaysRequired);
        break;
      case v2xpb::asn::MapLane_AllowedManeuver_GO_WITH_HALT:
        maneuvers.set_bit(AllowedManeuvers_goWithHalt);
        break;
      case v2xpb::asn::MapLane_AllowedManeuver_CAUTION:
        maneuvers.set_bit(AllowedManeuvers_caution);
        break;
      case v2xpb::asn::MapLane_AllowedManeuver_RESERVED:
        maneuvers.set_bit(AllowedManeuvers_reserved1);
        break;
      default:
        break;
    }
  }
  return true;
}

bool map_connection_asn2pb(const Connection_t *connection_asn,
                           ::v2xpb::asn::MapConnection *connection_pb) {
  if (!connection_asn || !connection_pb) {
    LOG(ERROR) << "NULLPTR";
    return false;
  }

  // ID
  if (!map_node_id_asn2pb(&connection_asn->remoteIntersection,
                          connection_pb->mutable_remote_node_id())) {
    LOG(ERROR) << "Failed to convert node id.";
    return false;
  }

  if (nullptr != connection_asn->phaseId) {
    connection_pb->set_phase_id(static_cast<int>(*connection_asn->phaseId));
  }
  if (nullptr != connection_asn->connectingLane) {
    auto con_lane = connection_asn->connectingLane->lane;
    connection_pb->set_lane_id(static_cast<int>(con_lane));
    if (nullptr != connection_asn->connectingLane->maneuver) {
      if (!map_allowed_maneuvers_asn2pb(
              connection_asn->connectingLane->maneuver,
              connection_pb->mutable_maneuvers())) {
        LOG(ERROR) << "Failed to convert maneuvers";
        return false;
      }
    }
  }
  return true;
}
//
bool map_connection_pb2asn(const ::v2xpb::asn::MapConnection &connection_pb,
                           Connection_t *connection_asn) {
  if (!connection_asn) {
    LOG(ERROR) << "NULLPTR";
    return false;
  }

  // ID
  if (!map_node_id_pb2asn(connection_pb.remote_node_id(),
                          &connection_asn->remoteIntersection)) {
    LOG(ERROR) << "Failed to convert node id.";
    return false;
  }

  if (connection_pb.has_phase_id()) {
    connection_asn->phaseId = ASN1(PhaseID)::create_tp();
    *connection_asn->phaseId = connection_pb.phase_id();
  }
  connection_asn->connectingLane = ASN1(ConnectingLane)::create_tp();
  connection_asn->connectingLane->lane = connection_pb.lane_id();
  connection_asn->connectingLane->maneuver =
      ASN1(AllowedManeuvers)::create_tp();

  if (!map_allowed_maneuvers_pb2asn(connection_pb.maneuvers(),
                                    connection_asn->connectingLane->maneuver)) {
    LOG(ERROR) << "Failed to convert maneuvers";
    return false;
  }
  return true;
}
//
bool map_lane_asn2pb(const Position3D_t *ref_position, const Lane_t *lane_asn,
                     ::v2xpb::asn::MapLane *lane_pb) {
  if (!ref_position || !lane_asn || !lane_pb) {
    LOG(ERROR) << "NULLPTR";
    return false;
  }
  lane_pb->set_id(static_cast<int>(lane_asn->laneID));
  if (lane_asn->laneWidth) {
    lane_pb->set_width(0.01 * static_cast<double>(*lane_asn->laneWidth));
  }
  if (nullptr != lane_asn->laneAttributes) {
    lane_attri_asn2pb(lane_asn->laneAttributes,
                      lane_pb->mutable_lane_attributes());
  }
  if (nullptr != lane_asn->speedLimits) {
    for (int i = 0; i < lane_asn->speedLimits->list.count; i++) {
      if (!speed_limit_asn2pb(lane_asn->speedLimits->list.array[i],
                              lane_pb->add_speed_limits())) {
        LOG(ERROR) << "Failed to convert speed limits";
        return false;
      }
    }
  }
  if (nullptr != lane_asn->maneuvers) {
    if (!map_allowed_maneuvers_asn2pb(lane_asn->maneuvers,
                                      lane_pb->mutable_maneuvers())) {
      LOG(ERROR) << "Failed to convert maneuvers";
      return false;
    }
  }
  if (nullptr != lane_asn->connectsTo) {
    for (int i = 0; i < lane_asn->connectsTo->list.count; i++) {
      if (!map_connection_asn2pb(lane_asn->connectsTo->list.array[i],
                                 lane_pb->add_connections())) {
        LOG(ERROR) << "Failed to convert connection";
        return false;
      }
    }
  }
  if (nullptr != lane_asn->points) {
    for (int i = 0; i < lane_asn->points->list.count; i++) {
      if (!position_asn2pb(ref_position,
                           &lane_asn->points->list.array[i]->posOffset,
                           lane_pb->add_positions())) {
        LOG(ERROR) << "Failed to convert position";
        return false;
      }
    }
  }
  return true;
}
//
bool map_lane_pb2asn(const Position3D_t *ref_position,
                     const ::v2xpb::asn::MapLane &lane_pb, Lane_t *lane_asn) {
  if (!lane_asn) {
    LOG(ERROR) << "NULLPTR";
    return false;
  }
  lane_asn->laneID = lane_pb.id();
  if (lane_pb.has_width()) {
    lane_asn->laneWidth = ASN1(LaneWidth)::create_tp();
    *lane_asn->laneWidth = static_cast<int>(round(100 * lane_pb.width()));
  }
  if (lane_pb.has_lane_attributes()) {
    lane_asn->laneAttributes = ASN1(LaneAttributes)::create_tp();
    lane_attri_pb2asn(lane_pb.lane_attributes(), lane_asn->laneAttributes);
  }

  for (const auto &sl : lane_pb.speed_limits()) {
    ASN1(SpeedLimitList)::create_tp(&lane_asn->speedLimits);
    ASN_SEQUENCE_ADD(&lane_asn->speedLimits->list,
                     ASN1(RegulatorySpeedLimit)::create_tp());
    if (!speed_limit_pb2asn(
            sl, lane_asn->speedLimits->list
                    .array[lane_asn->speedLimits->list.count - 1])) {
      LOG(ERROR) << "Failed to convert speed limits";
      return false;
    }
  }

  lane_asn->maneuvers = ASN1(AllowedManeuvers)::create_tp();
  if (!map_allowed_maneuvers_pb2asn(lane_pb.maneuvers(), lane_asn->maneuvers)) {
    LOG(ERROR) << "Failed to convert maneuvers";
    return false;
  }
  for (const auto &connection_pb : lane_pb.connections()) {
    ASN1(ConnectsToList)::create_tp(&lane_asn->connectsTo);
    ASN_SEQUENCE_ADD(&lane_asn->connectsTo->list,
                     ASN1(Connection)::create_tp());
    if (!map_connection_pb2asn(
            connection_pb, lane_asn->connectsTo->list
                               .array[lane_asn->connectsTo->list.count - 1])) {
      LOG(ERROR) << "Failed to convert connection";
      return false;
    }
  }
  for (const auto &point : lane_pb.positions()) {
    ASN1(PointList)::create_tp(&lane_asn->points);
    ASN_SEQUENCE_ADD(&lane_asn->points->list, ASN1(RoadPoint)::create_tp());
    if (!position_pb2asn(
            ref_position, point,
            &lane_asn->points->list.array[lane_asn->points->list.count - 1]
                 ->posOffset)) {
      LOG(ERROR) << "Failed to convert position";
      return false;
    }
  }
  return true;
}
//
bool map_link_asn2pb(const Position3D_t *ref_position, const Link_t *link_asn,
                     ::v2xpb::asn::MapLink *link_pb) {
  if (!ref_position || !link_asn || !link_pb) {
    LOG(ERROR) << "NULLPTR";
    return false;
  }

  // ID
  if (!map_node_id_asn2pb(&link_asn->upstreamNodeId,
                          link_pb->mutable_upstream_node_id())) {
    LOG(ERROR) << "Failed to convert node id.";
    return false;
  }

  // Name
  if (link_asn->name) {
    link_pb->set_name(
        std::string((const char *)link_asn->name->buf, link_asn->name->size));
  }

#if defined(CONFIG_ADAPTER_MODULE_asn_3_ENABLE)
  link_pb->set_width(0.01 * static_cast<double>(link_asn->linkWidth));
#elif defined(CONFIG_ADAPTER_MODULE_asn_2_ENABLE)
  if (nullptr != link_asn->linkWidth) {
    link_pb->set_width(0.01 * static_cast<double>(*link_asn->linkWidth));
  }
#endif
  for (int i = 0; i < link_asn->lanes.list.count; i++) {
    if (!map_lane_asn2pb(ref_position, link_asn->lanes.list.array[i],
                         link_pb->add_lanes())) {
      LOG(ERROR) << "Failed to convert map lane";
      return false;
    }
  }
  if (nullptr != link_asn->points) {
    for (int i = 0; i < link_asn->points->list.count; i++) {
      if (!position_asn2pb(ref_position,
                           &link_asn->points->list.array[i]->posOffset,
                           link_pb->add_positions())) {
        LOG(ERROR) << "Failed to convert position";
        return false;
      }
    }
  }
  if (link_pb->lanes().empty()) {
    LOG(ERROR) << "NO map lanes HERE!";
    return false;
  }

  if (nullptr != link_asn->speedLimits) {
    for (int i = 0; i < link_asn->speedLimits->list.count; i++) {
      if (!speed_limit_asn2pb(link_asn->speedLimits->list.array[i],
                              link_pb->add_speed_limits())) {
        LOG(ERROR) << "Failed to convert speed limits";
        return false;
      }
    }
  }

  if (nullptr != link_asn->movements) {
    for (int i = 0; i < link_asn->movements->list.count; i++) {
      if (!movement_asn2pb(link_asn->movements->list.array[i],
                           link_pb->add_movements())) {
        LOG(ERROR) << "Failed to convert speed limits";
        return false;
      }
    }
  }

  return true;
}
//
bool map_link_pb2asn(const Position3D_t *ref_position,
                     const ::v2xpb::asn::MapLink &link_pb, Link_t *link_asn) {
  if (!link_asn) {
    LOG(ERROR) << "NULLPTR";
    return false;
  }

  // ID
  if (!map_node_id_pb2asn(link_pb.upstream_node_id(),
                          &link_asn->upstreamNodeId)) {
    LOG(ERROR) << "Failed to convert node id.";
    return false;
  }

  // Name
  if (link_pb.has_name()) {
    link_asn->name = ASN1(DescriptiveName)::create_tp();
    OCTET_STRING_fromBuf(link_asn->name, link_pb.name().c_str(),
                         link_pb.name().length());
  }

#if defined(CONFIG_ADAPTER_MODULE_asn_3_ENABLE)
  link_asn->linkWidth = static_cast<int>(floor(link_pb.width() * 100));
#elif defined(CONFIG_ADAPTER_MODULE_asn_2_ENABLE)
  link_asn->linkWidth = ASN1(LaneWidth)::create_tp();
  *link_asn->linkWidth = static_cast<int>(floor(link_pb.width() * 100));
#endif
  for (const auto &lane_pb : link_pb.lanes()) {
    ASN_SEQUENCE_ADD(&link_asn->lanes.list, ASN1(Lane)::create_tp());
    if (!map_lane_pb2asn(
            ref_position, lane_pb,
            link_asn->lanes.list.array[link_asn->lanes.list.count - 1])) {
      LOG(ERROR) << "Failed to convert map lane";
      return false;
    }
  }
  for (const auto &point : link_pb.positions()) {
    ASN1(PointList)::create_tp(&link_asn->points);
    ASN_SEQUENCE_ADD(&link_asn->points->list, ASN1(RoadPoint)::create_tp());
    if (!position_pb2asn(
            ref_position, point,
            &link_asn->points->list.array[link_asn->points->list.count - 1]
                 ->posOffset)) {
      LOG(ERROR) << "Failed to convert position";
      return false;
    }
  }

  for (const auto &sl : link_pb.speed_limits()) {
    ASN1(SpeedLimitList)::create_tp(&link_asn->speedLimits);
    ASN_SEQUENCE_ADD(&link_asn->speedLimits->list,
                     ASN1(RegulatorySpeedLimit)::create_tp());
    if (!speed_limit_pb2asn(
            sl, link_asn->speedLimits->list
                    .array[link_asn->speedLimits->list.count - 1])) {
      LOG(ERROR) << "Failed to convert speed limits";
      return false;
    }
  }

  for (const auto &mv : link_pb.movements()) {
    ASN1(MovementList)::create_tp(&link_asn->movements);
    ASN_SEQUENCE_ADD(&link_asn->movements->list, ASN1(Movement)::create_tp());
    if (!movement_pb2asn(mv, link_asn->movements->list
                                 .array[link_asn->movements->list.count - 1])) {
      LOG(ERROR) << "Failed to convert speed limits";
      return false;
    }
  }

  return true;
}
//
bool map_node_asn2pb(const Node_t *node_asn, ::v2xpb::asn::MapNode *node_pb) {
  if (!node_asn || !node_pb) {
    LOG(ERROR) << "NULLPTR";
    return false;
  }

  // ID
  if (!map_node_id_asn2pb(&node_asn->id, node_pb->mutable_id())) {
    LOG(ERROR) << "Failed to convert node id.";
    return false;
  }

  // Name
  if (node_asn->name) {
    node_pb->set_name(
        std::string((const char *)node_asn->name->buf, node_asn->name->size));
  }

  // Position
  if (!position_asn2pb(&node_asn->refPos, node_pb->mutable_position())) {
    LOG(ERROR) << "Failed to convert position.";
    return false;
  }

  // Links
  if (nullptr != node_asn->inLinks) {
    for (int i = 0; i < node_asn->inLinks->list.count; i++) {
      if (!map_link_asn2pb(&node_asn->refPos, node_asn->inLinks->list.array[i],
                           node_pb->add_links())) {
        LOG(ERROR) << "Failed to convert map link";
        return false;
      }
    }
  }
  if (node_pb->links().empty()) {
    LOG(ERROR) << "NO map links HERE!";
    return false;
  }
  return true;
}
//
bool map_node_pb2asn(const ::v2xpb::asn::MapNode &node_pb, Node_t *node_asn) {
  if (!node_asn) {
    LOG(ERROR) << "NULLPTR";
    return false;
  }

  // ID
  if (!map_node_id_pb2asn(node_pb.id(), &node_asn->id)) {
    LOG(ERROR) << "Failed to convert node id.";
    return false;
  }

  // Name
  if (node_pb.has_name()) {
    node_asn->name = ASN1(DescriptiveName)::create_tp();
    OCTET_STRING_fromBuf(node_asn->name, node_pb.name().c_str(),
                         node_pb.name().length());
  }

  // Position
  if (!position_pb2asn(node_pb.position(), &node_asn->refPos)) {
    LOG(ERROR) << "Failed to convert position.";
    return false;
  }

  // Links
  for (const auto &link_pb : node_pb.links()) {
    ASN1(LinkList)::create_tp(&node_asn->inLinks);
    ASN_SEQUENCE_ADD(&node_asn->inLinks->list, ASN1(Link)::create_tp());
    if (!map_link_pb2asn(
            &node_asn->refPos, link_pb,
            node_asn->inLinks->list.array[node_asn->inLinks->list.count - 1])) {
      LOG(ERROR) << "Failed to convert map link";
      return false;
    }
  }
  return true;
}
//
bool map_asn2pb(const MapData_t *map_asn, ::v2xpb::asn::Map *map_pb) {
  if (!map_asn || !map_pb) {
    LOG(ERROR) << "NULLPTR";
    return false;
  }
  map_pb->set_message_count(static_cast<int>(map_asn->msgCnt));
  for (int i = 0; i < map_asn->nodes.list.count; i++) {
    if (!map_node_asn2pb(map_asn->nodes.list.array[i], map_pb->add_nodes())) {
      LOG(ERROR) << "Failed to convert map node";
      return false;
    }
  }
  return true;
}
//
bool map_pb2asn(const ::v2xpb::asn::Map &map_pb, MapData_t *map_asn) {
  if (!map_asn) {
    LOG(ERROR) << "NULLPTR";
    return false;
  }
  map_asn->msgCnt = map_pb.message_count();
  for (const auto &node_pb : map_pb.nodes()) {
    ASN_SEQUENCE_ADD(&map_asn->nodes.list, ASN1(Node)::create_tp());
    if (!map_node_pb2asn(
            node_pb,
            map_asn->nodes.list.array[map_asn->nodes.list.count - 1])) {
      LOG(ERROR) << "Failed to convert map node";
      return false;
    }
  }
  return true;
}
